#include "behavior_tree_policy.h"
#include "../../task/behavior_tree_cpp/custom_stateful_action_node.h"
#include "utils/thread/thread.h"
#include "../../task/behavior_tree_cpp/custom_sync_action_node.h"

#include<deque>
#include<future>
#include <optional>

namespace policy
{
class BehaviorTreePolicy::ControlProxy
{
public:
    friend class BehaviorTreePolicy;
    enum CommandType
    {
        PAUSE,
        RESUME,
        CANCEL
    };

    static std::string commandTypeToString(CommandType commandType)
    {
        switch (commandType)
        {
            case PAUSE:
                return "pause";
            case RESUME:
                return "resume";
            case CANCEL:
                return "cancel";
            default:
                return "unknown";
        }
    }

    struct Command
    {
        CommandType type;
        std::promise<void> prom;
        Command(CommandType type, std::promise<void> &&prom)
            : type(type),
              prom(std::move(prom))
        {}
    };
public:
    /**
     * @brief 处理pause、resume、cancel，会阻塞直至调用完成
     * 
     * @param type 
     */
    void process(CommandType type)
    {
        std::promise<void> prom;

        std::unique_lock<std::mutex> lk(mu_);
        deque_.emplace_back(type, std::move(prom));
        lk.unlock();
        cv_.notify_one();
    }

    /**
     * @brief 在一个给定的等待时间内尝试获取命令
     * 
     * @param timeout 
     * @return std::optional<Command> 
     */
    std::optional<Command> get_command(std::chrono::milliseconds timeout = std::chrono::milliseconds(5))
    {
        std::unique_lock<std::mutex> lk(mu_);
        if (cv_.wait_for(lk, timeout, [&]
        { return !deque_.empty(); }))
        {
            auto res = std::move(deque_.front());
            deque_.pop_front();
            return std::move(res);
        }
        else
        {
            //timeout之后队列仍然为空
            return {};
        }
    }
private:
    std::deque<Command> deque_;
    std::mutex mu_;
    std::condition_variable cv_;
};

BehaviorTreePolicy::BehaviorTreePolicy(const std::string &name)
    : AbstractPolicy(name), ctrl_proxy_(std::make_unique<ControlProxy>())
{
}
bool BehaviorTreePolicy::addTask(const task::AbstractTaskConstPtr &task)
{
    auto *policy_control_node = dynamic_cast<BT::ControlNode *>(policy_tree_node_.get());
    if (nullptr == policy_control_node)
    {
        EASE_ERROR("behavior tree control add task fail, current policy is null");
        return false;
    }
    BT::ActionNodeBase *node = createActionNode(task->getName(), {}, task);
    node_vector_.push_back(std::shared_ptr<BT::ActionNodeBase>(node));
    policy_control_node->addChild(node);
    return true;
}
bool BehaviorTreePolicy::insertTask(int index, const task::AbstractTaskConstPtr &task)
{
    auto *policy_control_node = dynamic_cast<BT::ControlNode *>(policy_tree_node_.get());
    if (nullptr == policy_control_node)
    {
        EASE_ERROR("behavior tree control insert task fail, current policy is null");
        return false;
    }
    BT::ActionNodeBase *node = createActionNode(task->getName(), {}, task);
    node_vector_.push_back(std::shared_ptr<BT::ActionNodeBase>(node));
    policy_control_node->insertChild(index, node);
    return true;
}
bool BehaviorTreePolicy::addPolicy(const AbstractPolicyConstPtr &policy)
{
    try
    {
        std::any data = policy->getPolicyData();
        if (!data.has_value())
        {
            EASE_ERROR("behavior tree control add policy fail, input policy is null");
            return false;
        }
        auto *policy_control_node = dynamic_cast<BT::ControlNode *>(policy_tree_node_.get());
        if (nullptr == policy_control_node)
        {
            EASE_ERROR("behavior tree control add policy fail, current policy is null");
            return false;
        }
        auto *tree_node = std::any_cast<BT::TreeNode *>(data);
        policy_vector_.push_back(policy.get());
        policy_control_node->addChild(tree_node);
        return true;
    }
    catch (std::exception &e)
    {
        EASE_ERROR("behavior tree [%s] control policy any case exception: %s", name_.c_str(), e.what());
        return false;
    }
}
bool BehaviorTreePolicy::insertPolicy(int index, const AbstractPolicyConstPtr &policy)
{
    try
    {
        std::any data = policy->getPolicyData();
        if (!data.has_value())
        {
            EASE_ERROR("behavior tree control insert policy fail, input policy is null");
            return false;
        }
        auto *policy_control_node = dynamic_cast<BT::ControlNode *>(policy_tree_node_.get());
        if (nullptr == policy_control_node)
        {
            EASE_ERROR("behavior tree control insert policy fail, current policy is null");
            return false;
        }
        auto *tree_node = std::any_cast<BT::TreeNode *>(data);
        policy_vector_.push_back(policy.get());
        policy_control_node->insertChild(index, tree_node);
        return true;
    }
    catch (std::exception &e)
    {
        EASE_ERROR("behavior tree [%s] control policy any case exception: %s", name_.c_str(), e.what());
        return false;
    }
}
bool BehaviorTreePolicy::setTask(const task::AbstractTaskConstPtr &task)
{
    auto *policy_decorator_node = dynamic_cast<BT::DecoratorNode *>(policy_tree_node_.get());
    if (nullptr == policy_decorator_node)
    {
        EASE_ERROR("behavior tree decorator set task fail, current policy is null");
        return false;
    }
    BT::ActionNodeBase *node = createActionNode(task->getName(), {}, task);
    node_vector_.push_back(std::shared_ptr<BT::ActionNodeBase>(node));
    policy_decorator_node->setChild(node);
    return true;
}
bool BehaviorTreePolicy::setPolicy(const AbstractPolicyConstPtr &policy)
{
    try
    {
        std::any data = policy->getPolicyData();
        if (!data.has_value())
        {
            EASE_ERROR("behavior tree decorator set policy fail, input policy is null");
            return false;
        }
        auto *policy_decorator_node = dynamic_cast<BT::DecoratorNode *>(policy_tree_node_.get());
        if (nullptr == policy_decorator_node)
        {
            EASE_ERROR("behavior tree decorator set policy fail, current policy is null");
            return false;
        }
        auto *tree_node = std::any_cast<BT::TreeNode *>(data);
        policy_vector_.push_back(policy.get());
        policy_decorator_node->setChild(tree_node);
        return true;
    }
    catch (std::exception &e)
    {
        EASE_ERROR("behavior tree decorator policy any case exception: %s", e.what());
        return false;
    }
}
std::any BehaviorTreePolicy::getPolicyData()
{
    return policy_tree_node_.get();
}
void BehaviorTreePolicy::run()
{
    bool pause_flag = false;
    BT::NodeStatus nodeStatus = policy_tree_node_->executeTick();
    while (BT::NodeStatus::RUNNING == nodeStatus)
    {
        //若是处于暂停状态，可以无限等待
        auto timeout = (pause_flag ? std::chrono::hours(48) : std::chrono::milliseconds(5));
        auto cmd = ctrl_proxy_->get_command(timeout);

        if (cmd.has_value())
        {
            EASE_DEBUG("%s get command, type: %d: %s", name_.c_str(), cmd->type, ControlProxy::commandTypeToString(cmd->type).c_str());
            auto &c = cmd.value();
            switch (c.type)
            {
                case ControlProxy::PAUSE:
                    if (!pause_flag)
                    {
                        policy_tree_node_->pause();
                        pause_flag = true;
                    }
                    c.prom.set_value();
                    break;
                case ControlProxy::RESUME:
                    if (pause_flag)
                    {
                        policy_tree_node_->resume();
                        pause_flag = false;
                    }
                    c.prom.set_value();
                    break;
                case ControlProxy::CANCEL:
                    pause_flag = false;
                    policy_tree_node_->cancel();
                    c.prom.set_value();
                    break;
                default:
                    EASE_WARN("get unknown command type: %d", cmd->type);
                    break;
            }
        }
        else
        {
            nodeStatus = policy_tree_node_->executeTick();
        }
    }
}
void BehaviorTreePolicy::pause()
{
    EASE_DEBUG("%s ctrl proxy process pause", name_.c_str());
    ctrl_proxy_->process(ControlProxy::PAUSE);
}
void BehaviorTreePolicy::resume()
{
    EASE_DEBUG("%s ctrl proxy process resume", name_.c_str());
    ctrl_proxy_->process(ControlProxy::RESUME);
}
void BehaviorTreePolicy::cancel()
{
    EASE_DEBUG("%s ctrl proxy process cancel", name_.c_str());
    ctrl_proxy_->process(ControlProxy::CANCEL);
}
void BehaviorTreePolicy::setContext(task::Context *context)
{
    for (auto &pNode: node_vector_)
    {
        auto node = dynamic_cast<task::node::AbstractCustomNode *>(pNode.get());
        if (node != nullptr)
        {
            node->getTask()->setContext(context);
        }
        else
        {
            EASE_ERROR("behavior tree policy [%s] set context node cast fail", name_.c_str());
        }
    }
    for (auto &policy: policy_vector_)
    {
        policy->setContext(context);
    }
}

BT::Optional<BT::NodeStatus> BehaviorTreePolicy::preTick(BT::TreeNode &tree_node, BT::NodeStatus node_status)
{
    return BT::NodeStatus::SUCCESS;
}
BT::Optional<BT::NodeStatus> BehaviorTreePolicy::postTick(BT::TreeNode &tree_node, BT::NodeStatus node_status, BT::NodeStatus new_status)
{
    return BT::NodeStatus::SUCCESS;
}
BT::ActionNodeBase *BehaviorTreePolicy::createActionNode(const std::string &name, const BT::NodeConfiguration &config,
                                                         const std::shared_ptr<task::AbstractTask> &task)
{
    task->setEventNotify(std::bind(&BehaviorTreePolicy::taskEventNotify, this, std::placeholders::_1));
    switch (task->getTaskType())
    {
        case task::TaskType::SYNC_TASK:
            return new task::node::CustomStatefulActionNode(task->getName(), {}, task);
        case task::TaskType::SYNC_REQUEST_TASK:
            return new task::node::CustomSyncActionNode(task->getName(), {}, task);
        default:
            EASE_ERROR("policy create action node error, unknown task type");
            return nullptr;
    }
}
std::string BehaviorTreePolicy::toString() const
{
    if (policy_tree_node_)
    {
        return policy_tree_node_->toString("- ");
    }
    return {};
}
BehaviorTreePolicy::~BehaviorTreePolicy() = default;

}
